﻿using System;
using System.IO.Ports;
using System.Collections.Generic;
using System.Threading.Tasks;
using TinyROS.Exceptions;
using TinyROS.Messages;
using System.Threading;

namespace TinyROS
{
    public class DiffCarNode : Node , IDisposable
    {
        private SerialPort ControllerPort;
        private CancellationTokenSource PortCanceller;
        private Subscriber<DiffcarCourseMessage> VelSubscriber;
        private Publisher<DiffcarVelocityMessage> MotorVelPublisher;
        private Dictionary<Publisher, string> pubedTopicName = new();
        private Dictionary<Subscriber, string> subedTopicName = new();
        public DiffCarNode() : 
            this(TinyROSConfig.DefaultTopicName.DiffcarCommand, 
                TinyROSConfig.DefaultTopicName.DiffcarVelocity)
        {

        }
        public DiffCarNode(string velSubTopicName,string velPubTopicName)
        {
            this.MotorVelPublisher = new();
            this.VelSubscriber = new(this.SetCourseFromMsg);
            this.VelSubscriber.Rate = 400;
            this.PublishedTopicName.Add(this.MotorVelPublisher, velPubTopicName);
            this.SubscribedTopicName.Add(this.VelSubscriber, velSubTopicName);
        }
        public override Dictionary<Publisher, string> PublishedTopicName { get => this.pubedTopicName; }
        public override Dictionary<Subscriber, string> SubscribedTopicName { get => this.subedTopicName; }
        private bool running;
        public override bool IsRunning => this.running;

        public void Dispose()
        {
            this.Stop();
            this.ControllerPort.Dispose();            
            this.SetCourse(0.0f, 99999f);
        }

        public override void Init(SystemCore master)
        {
            var pubVelTopic = master.RequestTopic<DiffcarVelocityTopic>(this.PublishedTopicName[this.MotorVelPublisher]);
            this.MotorVelPublisher.SetTopic(pubVelTopic);
            var velTopic = master.RequestTopic<DiffcarCourseTopic>(this.SubscribedTopicName[this.VelSubscriber]);
            this.VelSubscriber.SetTopic(velTopic);
            try
            {
                this.InitPort(TinyROSConfig.Stm32.Port.Name);
            }
            catch(ArgumentException)
            {
                throw new SerialPortInitException();
            }
            
            this.Initialized = true;
        }

        public override void Run()
        {
            if(this.Initialized)
            {
                this.VelSubscriber.Start();
                this.StartPort();
                this.running = true;
            }
            else
            {
                throw new NodeNotInitedException();
            }
        }
        public override void Stop()
        {
            this.VelSubscriber.Stop();           
            this.SetCourse(0.0f, 99999f);
            this.StopPort();
            this.running = false;
        }
        private void InitPort(string portName)
        {
            this.ControllerPort = new SerialPort(portName);
            this.ControllerPort.BaudRate = 19200;
            this.ControllerPort.DataReceived += this.SerialPortRxCallback;
            this.ControllerPort.Open();
        }
        private void StartPort()
        {
            
            this.PortCanceller = new();
            this.ListenPortAsync();
        }
        private void StopPort()
        {
            try
            {
                this.PortCanceller.Cancel();
            }
            catch
            {

            }
        }
        private async void ListenPortAsync()
        {
            try 
            {
                await Task.Run(async () =>
                {
                    while (true)
                    {
                        this.PortCanceller.Token.ThrowIfCancellationRequested();
                        var l = await this.ReadLineAsync();
                        try
                        {
                            float vl, vr;
                            var v = l.Split(' ');
                            vl = float.Parse(v[0]);
                            vr = float.Parse(v[1]);
                            //Console.WriteLine($"Received: vl = {vl},vr = {vr}");
                            DiffcarVelocity diffv = new DiffcarVelocity(vl, vr);
                            var diffvmsg = new DiffcarVelocityMessage(diffv, this);
                            this.MotorVelPublisher.Talk(diffvmsg);
                        }
                        catch (FormatException)
                        {
                            //Console.WriteLine("rx format exception");
                        }
                        catch (ArgumentException)
                        {
                            //Console.WriteLine("rx argument exception");
                        }
                    }
                }, this.PortCanceller.Token);
            } 
            catch (OperationCanceledException)
            {

            }
            catch
            {
                throw;
            }
        }
        private void SetCourseFromMsg(DiffcarCourseMessage msg)
        {
            var course = msg.Value;
            //Console.WriteLine($"Try to set vel:{course.Linear},raduis:{course.TurningRadius}");
            this.SetCourse(course.Linear, course.TurningRadius);
        }
        private void SetCourse(float linear, float radius)
        {
            var rmm = MathF.Abs(radius * 1000);
            var ctr = 0;
            if (rmm < 9999f)
            {
                if (radius <= 0.0f)
                {
                    ctr = 1;
                }
                else
                {
                    ctr = 2;
                }
            }
            var l = ((int)(linear * 1000)) % 1000;
            var r = (int)rmm % 10000;
            string cmd = string.Format("{0:d3}{1}{2:d4}", l, ctr, r);
            //Console.WriteLine("Port Sent {0}",cmd);
            this.ControllerPort.Write(cmd);
        }
        private void SerialPortRxCallback(object sender, SerialDataReceivedEventArgs e)
        {

        }
        private async Task<string> ReadLineAsync()
        {
            var line = await Task.Run(() => { return this.ControllerPort.ReadLine(); });
            return line;
        }
    }
}
